﻿#include "ethernet-parser.h"

using namespace eth_parser;

Bool EthernetParser::parse(Binary& rawPacket, UInt channel, Array<BusChannelType>& typeHints, BusChannelType& type, UInt& id)
{
    if (channel <= typeHints.size())
    {
        type = typeHints[channel - 1];
        return parseDirectly(rawPacket, type, id);
    }

    UInt rawSize = rawPacket.size();
    if (rawSize <= 14 || rawSize > 65535) return FALSE; // invalid for normal ethernet packet

    type = BusChannelType::Ethernet;
    id = *(UInt*)&rawPacket[8]; // idByMac
    return TRUE;
}

Bool EthernetParser::parseDirectly(Binary& rawPacket, BusChannelType type, UInt& id)
{
    UInt rawSize = rawPacket.size();
    if (rawSize <= 14 || rawSize > 65535) return FALSE; // invalid for normal ethernet packet

    Byte *rawData = rawPacket.data();

    if (type == BusChannelType::Ethernet)
    {
        id = *(UInt*)&rawData[8]; // idByMac
        return TRUE;
    }
    else if (type == BusChannelType::SomeIP)
    {
        UInt someIPStartIndex = 0;
        if (parseSomeIP(rawData, rawSize, someIPStartIndex)) id = parseBigEndianUInt(rawData, someIPStartIndex);
        else id = 0;
        return TRUE;
    }
    else return FALSE;
}

Word EthernetParser::parseBigEndianWord(Byte *rawData, UInt offset)
{
    return ((UInt)rawData[offset] << 8) | (UInt)rawData[offset + 1];
}

UInt EthernetParser::parseBigEndianUInt(Byte *rawData, UInt offset)
{
    return ((UInt)rawData[offset] << 24) | ((UInt)rawData[offset + 1] << 16) | ((UInt)rawData[offset + 2] << 8) | (UInt)rawData[offset + 3];
}

Bool EthernetParser::parseSomeIP(Byte *rawData, UInt rawSize, UInt& someIPStartIndex)
{
    UInt ipv4StartIndex = 0;
    if (rawData[12] == 8 && rawData[13] == 0) // IPv4
    {
        ipv4StartIndex = 14;
    }
    else if (rawData[12] == 0x81 && rawData[13] == 0) // VLAN tag
    {
        if (rawSize <= 18) return FALSE;
        if (rawData[16] == 8 && rawData[17] == 0) // IPv4
        {
            ipv4StartIndex = 18;
        }
    }

    if (ipv4StartIndex != 0)
    {
        if (rawSize <= ipv4StartIndex + 20) return FALSE;
        
        UInt ipLayerLength = parseBigEndianWord(rawData, ipv4StartIndex + 2);
        if (rawSize < ipv4StartIndex + ipLayerLength) return FALSE;

        if (rawData[ipv4StartIndex + 9] == 17) // UDP
        {
            UInt udpStartIndex = ipv4StartIndex + 20;

            if (rawSize <= udpStartIndex + 8) return FALSE;

            UInt udpLayerLength = parseBigEndianWord(rawData, udpStartIndex + 4);
            if (rawSize < udpStartIndex + udpLayerLength) return FALSE;

            someIPStartIndex = udpStartIndex + 8;

            if (rawSize <= someIPStartIndex + 16) return FALSE;

            UInt someIPLayerLength = parseBigEndianUInt(rawData, someIPStartIndex + 4);
            if (rawSize != someIPStartIndex + 8 + someIPLayerLength) return FALSE;

            if (rawData[someIPStartIndex] == 0xff && rawData[someIPStartIndex + 1] == 0xff) return FALSE;
            if (rawData[someIPStartIndex + 12] != 1 || rawData[someIPStartIndex + 14] != 2 || rawData[someIPStartIndex + 15] != 0) return FALSE;

            return TRUE;
        }
        else return FALSE;
    }
    else return FALSE;
}